/*

 * navigation.h
 *
 *  Created on: Nov 19, 2012
 *      Author: robot
 */

#ifndef NAVIGATION_H_
#define NAVIGATION_H_

#include "ros/ros.h"
#include <behavior/behavior.h>
#include <driver/Move.h>
#include <SLAM/Location.h>
#include <SLAM/RecordedTags.h>
#include <SLAM/Waypoints.h>
#include <sensor/Snapshot.h>


class Navigation : public Behavior
{
	struct	Waypoint {
		double	x;
		double y;
	};

	struct Tag {
		double x;
		double y;
		int		index;
	};

public:
					Navigation			(ros::NodeHandle n);
	bool 			act 				(ros::Publisher publisher);
	void 			setActive 			(bool state);

private:
	ros::NodeHandle 		nodeHandle;
	ros::ServiceClient 		tag_client;
	ros::ServiceClient		location_client;
	ros::ServiceClient 		waypoint_client;
	ros::ServiceClient 		snapshot_client;

	std::vector<Waypoint>	waypoints;
	std::vector<Tag> 		tags;

	void 					loadWaypointsForTag (int tagIndex);
	bool 					selectWaypoint 		();
	bool					selectNextWaypoint	();
	void					moveToTarget 		(driver::Move * move);
	void					standStill 			(driver::Move * move);
	Waypoint				target;
	bool					targetInitialized;
	bool					goingToStart;
	bool					takeSnapshot;

	// Location of the robot
	void 					updateLocation 		();
	float					x, y, angle;

	// Take a high-resolution image with the camera.
	void 					takeCamshot				();
};




#endif /* NAVIGATION_H_ */
